#include "StdAfx.h"
#include <bullet/BulletCollision/CollisionShapes/btTriangleMesh.h>
#include "..\engine\BulletTriangleMeshConvert.hpp"

namespace he
{
	namespace
	{
		//! Generic data converter.
		template<typename T>
		class DataConvert : boost::noncopyable
		{
			// Attributes
		private:
			T *m_pData;

			// Operations
		public:
			//! Get next value.
			unsigned int get_next();

			// Construction/Destruction
		public:
			DataConvert(void *pData);
		};

		template<typename T>
		DataConvert<T>::DataConvert(void *pData)
			: m_pData(static_cast<T *>(pData))
		{
		}

		template<typename T>
		unsigned int DataConvert<T>::get_next()
		{
			return *(m_pData++);
		}
	}

	BulletTriangleMeshPtr_t BulletTriangleMeshConvert::convert(Ogre::VertexData const &VertexData, Ogre::IndexData const &IndexData)
	{
		BulletTriangleMeshPtr_t p_bullet_trimesh(new btTriangleMesh(false, true));
		// Get the positional buffer element
		Ogre::VertexElement const * const pos_elems = VertexData.vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);			
		Ogre::HardwareVertexBufferSharedPtr p_vbuf = VertexData.vertexBufferBinding->getBuffer(pos_elems->getSource());
		// Get the index buffer
		Ogre::HardwareIndexBufferSharedPtr p_ibuf = IndexData.indexBuffer;	
		bool const use_32bit_indexes = (p_ibuf->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);
		void *p_index_data = p_ibuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY);
		// Create helper class
		DataConvert<Ogre::uint32> data_32(p_index_data);
		DataConvert<Ogre::uint16> data_16(p_index_data);
		// Get the vertex stride
		size_t const vertex_size = p_vbuf->getVertexSize();

		unsigned char* p_vertices = static_cast<unsigned char*>(p_vbuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
		size_t const num_faces = IndexData.indexCount / 3;
		btVector3 vertexPos[3];
		for(size_t i = 0; i < num_faces; ++i)
		{
			for(unsigned short j = 0; j < 3; ++j)
			{
				unsigned int const indxe_to_use = (use_32bit_indexes ? data_32.get_next() : data_16.get_next());
				float* p_element;
				pos_elems->baseVertexPointerToElement(p_vertices + indxe_to_use * vertex_size, &p_element);
				
				vertexPos[j].setValue(*p_element, *(p_element + 1), *(p_element + 2));
			}

			p_bullet_trimesh->addTriangle(vertexPos[0], vertexPos[1], vertexPos[2]);
		}

		p_vbuf->unlock();
		p_ibuf->unlock();

		return p_bullet_trimesh;
	}
} /*he*/